﻿using System;
using System.Linq;
using System.Collections.Generic;

using Microsoft.Xna.Framework;

using Ah.Testudin.Engine.Actors;
using Ah.Testudin.Engine.Graphics.Scenes;
using Ah.Testudin.Engine.Actors.Components;
using Ah.Testudin.Engine.Events;

namespace Ah.Testudin.Engine.Physics
{
    public class BulletSharpPhysicsSimulator : IPhysicsSimulator
    {
        #region Private Member Variables

        private const int MAX_STEPS = 4;

        private BulletSharp.DynamicsWorld _dynamicsWorld;

        private BulletSharp.CollisionConfiguration _collisionConfiguration;

        private BulletSharp.CollisionDispatcher _collisionDispatcher;

        private BulletSharp.BroadphaseInterface _broadphase;

        private BulletSharp.ConstraintSolver _constraintSolver;

        private IDictionary<ActorId, BulletSharp.RigidBody> _actorToRigidBodyMap;

        private HashSet<CollisionPair> _previousTickCollisionPairs;

        private readonly IEventManager _eventManager;

        private readonly BulletSharp.IDebugDraw _debugDrawer;

        #endregion

        #region Constructors

        public BulletSharpPhysicsSimulator(BulletSharp.IDebugDraw debugDrawer)
        {
            _eventManager = EventManagerSingleton.Instance;
        }

        #endregion

        #region IPhysicsSimulator

        public GetActorDelegate GetActorDelegate { private get;  set; }

        public void Initialize()
        {
            _collisionConfiguration = new BulletSharp.DefaultCollisionConfiguration();

            _collisionDispatcher = new BulletSharp.CollisionDispatcher(_collisionConfiguration);

            _broadphase = new BulletSharp.DbvtBroadphase();

            _constraintSolver = new BulletSharp.SequentialImpulseConstraintSolver();

            _dynamicsWorld = new BulletSharp.DiscreteDynamicsWorld(
                _collisionDispatcher,
                _broadphase,
                _constraintSolver,
                _collisionConfiguration);

            _dynamicsWorld.DebugDrawer = _debugDrawer;

            _dynamicsWorld.SetInternalTickCallback(HandleInternalTickCallback);

            _dynamicsWorld.WorldUserInfo = this;

            _dynamicsWorld.Gravity = new Vector3(0, -.4f, 0);

            _actorToRigidBodyMap = new Dictionary<ActorId, BulletSharp.RigidBody>();

            _previousTickCollisionPairs = new HashSet<CollisionPair>();
        }

        public void Update(GameTime gameTime)
        {
            if (this.GetActorDelegate == null)
            {
                throw new Exception("Need a GetActorDelegate");
            }

            _dynamicsWorld.StepSimulation(
                (float)gameTime.ElapsedGameTime.TotalMilliseconds,
                MAX_STEPS);

            foreach (var actorRigidBody in _actorToRigidBodyMap)
            {
                ActorMotionState actorMotionState =
                    actorRigidBody.Value.MotionState as ActorMotionState;

                if (actorMotionState == null)
                {
                    throw new Exception("Expected ActorMotionState");
                }

                Actor actor = this.GetActorDelegate(actorRigidBody.Key);

                if (actor != null)
                {
                    TransformComponent transformComponent = actor.GetComponent<TransformComponent>();

                    if (transformComponent != null)
                    {
                        if (transformComponent.Transform != actorMotionState.WorldTransform)
                        {
                            transformComponent.Transform = actorMotionState.WorldTransform;

                            _eventManager.QueueEvent(
                                new ActorMovedEvent(
                                    actor.Id,
                                    actorMotionState.WorldTransform));
                        }
                    }
                }
            }
        }

        public void AddSphere(
            float radius,
            float gravityAcceleration,
            Actor actor,
            Matrix initialTransform)
        {
            BulletSharp.SphereShape collisionShape = new BulletSharp.SphereShape(radius);

            float volume = (4.0f / 3.0f) * MathHelper.Pi * radius * radius * radius;

            float mass = volume * gravityAcceleration;

            AddShape(
                actor,
                collisionShape,
                mass);
        }

        public void AddCube(
            Vector3 dimensions,
            float gravityAcceleration,
            Actor actor)
        {
            BulletSharp.BoxShape boxShape = new BulletSharp.BoxShape(dimensions);

            float volume = dimensions.X * dimensions.Y * dimensions.Z;

            float mass = volume * gravityAcceleration;

            AddShape(
                actor,
                boxShape,
                mass);
        }

        public void AddCapsule(
            Vector3 dimensions,
            float gravityAcceleration,
            Actor actor)
        {
            BulletSharp.CapsuleShape capsuleShape = new BulletSharp.CapsuleShape(
                dimensions.X,
                dimensions.Y);

            float volume = MathHelper.Pi *
                           dimensions.X * dimensions.X * 
                           dimensions.Y;

            float mass = volume * gravityAcceleration;

            AddShape(
                actor,
                capsuleShape,
                mass);
        }

        public void AddPointCloud(
            IEnumerable<Vector3> vertices,
            float gravityAcceleration,
            int numPoints,
            Actor actor)
        {
            BulletSharp.ConvexHullShape convexHullShape = new BulletSharp.ConvexHullShape();

            foreach (Vector3 currentVector in vertices)
            {
                convexHullShape.AddPoint(currentVector);
            }

            Vector3 aabbMin = Vector3.Zero;

            Vector3 aabbMax = Vector3.Zero;

            convexHullShape.GetAabb(
                Matrix.Identity,
                out aabbMin,
                out aabbMax);

            Vector3 aabbExtents = aabbMax - aabbMin;

            float volume = aabbExtents.X * aabbExtents.Y * aabbExtents.Z;

            float mass = volume * gravityAcceleration;

            AddShape(
                actor,
                convexHullShape,
                mass);
        }

        public void CreateTrigger(
            Actor actor,
            Vector3 position,
            float dimension)
        {
            BulletSharp.BoxShape boxShape = new BulletSharp.BoxShape(
                new Vector3(
                    dimension,
                    dimension,
                    dimension));

            Matrix transform = Matrix.CreateTranslation(position);

            ActorMotionState actorMotionState = new ActorMotionState(transform);

            BulletSharp.RigidBodyConstructionInfo constructionInfo = new BulletSharp.RigidBodyConstructionInfo(
                0,
                actorMotionState,
                boxShape,
                Vector3.Zero);

            BulletSharp.RigidBody rigidBody = new BulletSharp.RigidBody(constructionInfo);

            _dynamicsWorld.AddRigidBody(rigidBody);

            rigidBody.CollisionFlags = BulletSharp.CollisionFlags.NoContactResponse;

            _actorToRigidBodyMap.Add(
                actor.Id,
                rigidBody);
        }

        public void RemoveActor(ActorId actorId)
        {
            KeyValuePair<ActorId, BulletSharp.RigidBody> keyValuePair = _actorToRigidBodyMap.SingleOrDefault(
                x => x.Key == actorId);

            if (keyValuePair.Key != null)
            {
                RemoveCollisionObject(keyValuePair.Value);

                _actorToRigidBodyMap.Remove(actorId);
            }
        }

        public void ApplyForce(
            Vector3 direction,
            float newtons,
            ActorId actorId)
        {
            BulletSharp.RigidBody rigidBody = FindActorRigidBody(actorId);

            if (rigidBody != null)
            {
                rigidBody.ActivationState = BulletSharp.ActivationState.DisableDeactivation;

                Vector3 force = new Vector3(
                    direction.X * newtons,
                    direction.Y * newtons,
                    direction.Z * newtons);

                rigidBody.ApplyCentralImpulse(force);
            }
        }

        public void ApplyTorque(
            Vector3 direction,
            float magnitude,
            ActorId actorId)
        {
            BulletSharp.RigidBody rigidBody = FindActorRigidBody(actorId);

            if (rigidBody != null)
            {
                rigidBody.ActivationState = BulletSharp.ActivationState.DisableDeactivation;

                Vector3 torque = new Vector3(
                    direction.X * magnitude,
                    direction.Y * magnitude,
                    direction.Z * magnitude);

                rigidBody.ApplyTorqueImpulse(torque);
            }
        }

        public bool KinematicMove(
            ActorId actorId,
            Matrix newTransform)
        {
            BulletSharp.RigidBody rigidBody = FindActorRigidBody(actorId);

            if (rigidBody != null)
            {
                rigidBody.ActivationState = BulletSharp.ActivationState.DisableDeactivation;

                rigidBody.WorldTransform = newTransform;

                return true;
            }

            return false;
        }

        public Vector3 GetVelocity(ActorId actorId)
        {
            BulletSharp.RigidBody rigidBody = FindActorRigidBody(actorId);

            return rigidBody.LinearVelocity;
        }

        public void SetVelocity(
            ActorId actorId,
            Vector3 velocity)
        {
            BulletSharp.RigidBody rigidBody = FindActorRigidBody(actorId);

            rigidBody.ActivationState = BulletSharp.ActivationState.DisableDeactivation;

            rigidBody.LinearVelocity = velocity;
        }

        public void RenderDiagnostics()
        {
            _dynamicsWorld.DebugDrawWorld();
        }

        #endregion

        #region Private Methods

        private void HandleInternalTickCallback(
            BulletSharp.DynamicsWorld dynamicsWorld, 
            float timeStep)
        {
            HashSet<CollisionPair> currentTickCollisionPairs = new HashSet<CollisionPair>();

            BulletSharp.Dispatcher dispatcher = _dynamicsWorld.Dispatcher;

            for (int currentManifoldIndex = 0; currentManifoldIndex < dispatcher.NumManifolds; currentManifoldIndex++)
            {
                BulletSharp.PersistentManifold currentManifold = dispatcher.GetManifoldByIndexInternal(currentManifoldIndex);

                BulletSharp.RigidBody body1 = (BulletSharp.RigidBody)currentManifold.Body0;

                BulletSharp.RigidBody body2 = (BulletSharp.RigidBody)currentManifold.Body1;

                CollisionPair currentCollisionPair = new CollisionPair(
                    body1,
                    body2);

                currentTickCollisionPairs.Add(currentCollisionPair);

                if (_previousTickCollisionPairs.Contains(currentCollisionPair) == false)
                {
                    SendCollisionPairAdditionEvent(
                        currentManifold, 
                        currentCollisionPair);
                }
            }

            IEnumerable<CollisionPair> separatedCollisionPairs = _previousTickCollisionPairs.Except(currentTickCollisionPairs);

            foreach (CollisionPair currentSeparation in separatedCollisionPairs)
            {
                SendCollisionPairSeparationEvent(currentSeparation);
            }

            _previousTickCollisionPairs = currentTickCollisionPairs;
        }

        private void AddShape(
            Actor actor,
            BulletSharp.CollisionShape collisionShape,
            float mass)
        {
            KeyValuePair<ActorId, BulletSharp.RigidBody> keyValuePair =
                _actorToRigidBodyMap.SingleOrDefault(x => x.Key == actor.Id);

            if (keyValuePair.Key != null)
            {
                throw new Exception("Actor can't have more than 1 rigid body");
            }

            Vector3 localInertia = (mass > 0.0f)
                ? collisionShape.CalculateLocalInertia(mass)
                : Vector3.Zero;

            TransformComponent transformComponent = actor.GetComponent<TransformComponent>();

            if (transformComponent == null)
            {
                throw new Exception("Actor must have transform component to work with physics system.");
            }

            Matrix transform = transformComponent.Transform;

            ActorMotionState actorMotionState = new ActorMotionState(transform);

            BulletSharp.RigidBodyConstructionInfo rigidBodyConstructionInfo = 
                new BulletSharp.RigidBodyConstructionInfo(
                    mass,
                    actorMotionState,
                    collisionShape,
                    localInertia);

            BulletSharp.RigidBody rigidBody = new BulletSharp.RigidBody(rigidBodyConstructionInfo);

            _dynamicsWorld.AddRigidBody(rigidBody);

            _actorToRigidBodyMap.Add(
                new KeyValuePair<ActorId, BulletSharp.RigidBody>(
                    actor.Id,
                    rigidBody));
        }

        private BulletSharp.RigidBody FindActorRigidBody(ActorId actorId)
        {
            KeyValuePair<ActorId, BulletSharp.RigidBody> keyValuePair = _actorToRigidBodyMap.SingleOrDefault(
                x => x.Key == actorId);

            return keyValuePair.Value;
        }

        private ActorId FindRigidBodyActor(BulletSharp.RigidBody rigidBody)
        {
            KeyValuePair<ActorId, BulletSharp.RigidBody> keyValuePair = _actorToRigidBodyMap.SingleOrDefault(
                x => x.Value == rigidBody);

            return keyValuePair.Key;
        }

        private void RemoveCollisionObject(BulletSharp.CollisionObject collisionObject)
        {
            _dynamicsWorld.RemoveCollisionObject(collisionObject);
        }

        private void SendCollisionPairAdditionEvent(
            BulletSharp.PersistentManifold manifold,
            CollisionPair collisionPair)
        {
            if ((collisionPair.Body1.UserObject != null) ||
                (collisionPair.Body2.UserObject != null))
            {
                BulletSharp.RigidBody triggerBody, otherBody;

                if (collisionPair.Body1.UserObject != null)
                {
                    triggerBody = collisionPair.Body1;
                    otherBody = collisionPair.Body2;
                }
                else
                {
                    otherBody = collisionPair.Body1;
                    triggerBody = collisionPair.Body2;
                }

                int triggerId = (int)triggerBody.UserObject;

                _eventManager.QueueEvent(
                    new AreaEnteredEvent(
                        triggerId,
                        FindRigidBodyActor(otherBody)));
            }
            else
            {
                ActorId actorId1 = FindRigidBodyActor(collisionPair.Body1);

                ActorId actorId2 = FindRigidBodyActor(collisionPair.Body2);

                if ((actorId1 == null) ||
                    (actorId2 == null))
                {
                    return;
                }

                IList<Vector3> collisionPoints = new List<Vector3>();

                Vector3 sumNormalForce = Vector3.Zero;

                Vector3 sumFrictionForce = Vector3.Zero;

                for (int pointIndex = 0; pointIndex < manifold.NumContacts; pointIndex++)
                {
                    BulletSharp.ManifoldPoint currentContactPoint = manifold.GetContactPoint(pointIndex);

                    collisionPoints.Add(currentContactPoint.PositionWorldOnB);

                    sumNormalForce += 
                        currentContactPoint.CombinedRestitution * 
                        currentContactPoint.NormalWorldOnB;

                    sumFrictionForce +=
                        currentContactPoint.CombinedFriction *
                        currentContactPoint.LateralFrictionDir1;
                }

                _eventManager.QueueEvent(
                    new CollisionEvent(
                        actorId1,
                        actorId2,
                        sumNormalForce,
                        sumFrictionForce,
                        collisionPoints));
            }
        }

        private void SendCollisionPairSeparationEvent(CollisionPair collisionPair)
        {
            if ((collisionPair.Body1.UserObject != null) ||
                (collisionPair.Body2.UserObject != null))
            {
                BulletSharp.RigidBody triggerBody, otherBody;

                if (collisionPair.Body1.UserObject != null)
                {
                    triggerBody = collisionPair.Body1;
                    otherBody = collisionPair.Body2;
                }
                else
                {
                    otherBody = collisionPair.Body1;
                    triggerBody = collisionPair.Body2;
                }

                int triggerId = (int)triggerBody.UserObject;

                _eventManager.QueueEvent(
                    new AreaExitedEvent(
                        triggerId,
                        FindRigidBodyActor(otherBody)));
            }
            else
            {
                ActorId actorId1 = FindRigidBodyActor(collisionPair.Body1);

                ActorId actorId2 = FindRigidBodyActor(collisionPair.Body2);

                if ((actorId1 == null) ||
                    (actorId2 == null))
                {
                    return;
                }

                _eventManager.QueueEvent(
                    new CollisionSeparationEvent(
                        actorId1,
                        actorId2));
            }
        }

        #endregion
    }
}
